#include <iostream>
#include <string>
#include <sstream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace std; 

const string OUT_DIR = ""; 

class SimpleOpenNIViewer 
{ 
public: 
	
    bool save_one; 


    SimpleOpenNIViewer () : viewer ("PCL Viewer") 
    { 
                frames_saved = 0; 
                save_one = false; 
    } 

    void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) 
    { 
                if (!viewer.wasStopped()) { 
                       // viewer.showCloud (cloud); 

                        if( save_one ) { 
                                save_one = false; 
								cout << "Saving frame\n";                           
                                std::stringstream out; 
                                out << frames_saved; 
                                std::string name = OUT_DIR + "loopScene.pcd"; 
                                pcl::io::savePCDFileASCII( name, *cloud );

							char buf[BUFSIZ];
							size_t size;

							FILE* source = fopen("loopScene.pcd", "rb");
							FILE* dest = fopen("scene.pcd", "wb");

							if(dest != NULL){
								// clean and more secure
								// feof(FILE* stream) returns non-zero if the end of file indicator for stream is set

								while (size = fread(buf, 1, BUFSIZ, source)) {
									fwrite(buf, 1, size, dest);
								}
							}
							fclose(source);
							fclose(dest);

                        } 
                } 
    } 

    void run () 
    { 
                pcl::Grabber* interface = new pcl::OpenNIGrabber(); 

                boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = 
                        boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1); 

                interface->registerCallback (f); 

                interface->start (); 
				 
                char c; 

                while (!viewer.wasStopped()) 
                { 
                        //sleep (1); 

                        //c = getchar(); 
                       /* if( true ) { 
								Sleep(1000);
                                cout << "Saving frame " << frames_saved << ".\n"; 
                                frames_saved++; 
                                save_one = true; 
                        } */
                } 

                interface->stop (); 
        } 

        pcl::visualization::CloudViewer viewer; 

        private: 
                int frames_saved; 

};


void task(SimpleOpenNIViewer* v){

	v->run();

}

int main () 
{ 
    SimpleOpenNIViewer v; 
    //v.run (); 
	boost::thread t = boost::thread(task, &v);

	std::cout << "Aspetto 10 secondi" << std::endl;
	Sleep(10000);
	std::cout << "Provo" << std::endl;
	v.save_one = true;
	Sleep(10000);
	//t.join();
    return 0; 
} 